#include "includes.h"

#define MAX_GAS 250
#define MIN_GAS    15

#define sin45 sin(45.F / 180.F * PI) 
#define cos45 cos(45.F / 180.F * PI)
#define GYRO_ROLL 0
#define GYRO_NICK 1
#define GYRO_GIER 2
#define ACC_ROLL 3
#define ACC_NICK 4
#define ACC_GIER 5


float NeutralNick = 0.0F, NeutralRoll = 0.0F, NeutralGier = 0.0F;
s8 NeutralAccX = 0, NeutralAccY = 0, NeutralAccZ = 0;

volatile float AverageRoll = 0, AverageNick = 0, AverageGier = 0; 
volatile float AverageAccRoll=0,AverageAccNick=0,AverageAccHoch=0;

s8 AveragerACC_X = 0, AveragerACC_Y = 0, AveragerACC_Z = 0;

f32_t Roll_X_Offset = 0.F, Roll_Y_Offset = 0.F, Roll_Z_Offset = 0.F; 
f32_t ACC_X_Offset = 0.F, ACC_Y_Offset = 0.F,  ACC_Z_Offset = 0.F; 

s8 SummeNick=0,SummeRoll=0, StickNick = 0,StickRoll = 0,StickGier = 0, sollGier = 0;
s8 GierMischanteil, GasMischanteil;

void SetNeutral(void)
{

  OSTimeDlyHMSM(0,0,0,500);
  NeutralRoll = ConvertToVoltage(ADC_DMA_Filter(GYRO_ROLL));
  NeutralNick = ConvertToVoltage(ADC_DMA_Filter(GYRO_NICK));
  NeutralGier = ConvertToVoltage(ADC_DMA_Filter(GYRO_GIER));
  NeutralAccX = MMA7660_ConvertAcc(MMA7660_read(MMA7660_XOUT));
  NeutralAccY = MMA7660_ConvertAcc(MMA7660_read(MMA7660_YOUT));
  NeutralAccZ = MMA7660_ConvertAcc(MMA7660_read(MMA7660_ZOUT));

}


void GetMeasurements(void )
{
	
	AverageRoll=(ConvertToVoltage(ADC_DMA_Filter(GYRO_ROLL))-NeutralRoll)*2*PI*1000/(360*0.67);
	AverageNick=(ConvertToVoltage(ADC_DMA_Filter(GYRO_NICK))-NeutralNick)*2*PI*1000/(360*0.67);
	AverageGier=(ConvertToVoltage(ADC_DMA_Filter(GYRO_GIER))-NeutralGier)*2*PI*1000/(360*0.67);

	AverageAccNick=MMA7660_ConvertAcc(MMA7660_read(MMA7660_XOUT));//-NeutralAccX;
	AverageAccRoll=MMA7660_ConvertAcc(MMA7660_read(MMA7660_YOUT));//-NeutralAccY;
	AverageAccHoch=MMA7660_ConvertAcc(MMA7660_read(MMA7660_ZOUT));//-NeutralAccZ;

}



